#!/usr/bin/env python3
"""
@Author: wujin_xueyk\n
@Date:   2022-01-01\n
@Function: px4 drone sdk\n
"""
import sys
import time
import math
import threading

from .utils.rotation_transform import *
from .utils.util import *

from .px4_common import *
from .fcu_px4 import *


class DroneAPI(object):
    """
    Drone API class. 
    """

    def __init__(self, params_dict={}):

        self.params_dict = params_dict
        if 'fcu_type' in params_dict.keys():
            self.fcu_type = params_dict['fcu_type']
        else:
            self.fcu_type = 'px4'

        if self.fcu_type == 'px4':
            if 'sys_id' in params_dict:
                self.sys_id = params_dict['sys_id']
            else:
                self.sys_id = 1
                self.params_dict['sys_id'] = self.sys_id
            if 'cmp_id' in params_dict:
                self.cmp_id = params_dict['cmp_id']
            else:
                self.cmp_id = 1
                self.params_dict['cmp_id'] = self.cmp_id

            if 'tgt_sys_id' in params_dict:
                self.tgt_sys_id = params_dict['tgt_sys_id']
            else:
                self.tgt_sys_id = 1
                self.params_dict['tgt_sys_id'] = self.tgt_sys_id
            if 'tgt_cmp_id' in params_dict:
                self.tgt_cmp_id = params_dict['tgt_cmp_id']
            else:
                self.tgt_cmp_id = 1
                self.params_dict['tgt_cmp_id'] = self.tgt_cmp_id

            if 'fcu_url' in self.params_dict:
                self.fcu_url = self.params_dict['fcu_url']
            else:
                self.fcu_url = 'udp://:14550@127.0.0.1:18570'
                self.params_dict['fcu_url'] = self.fcu_url

            try:
                self.fcu = FcuPx4(self.fcu_url, self.params_dict)
            except Exception as e:
                print("ERROR: cannot initialize mavlink api, {}".format(e))
                return
        #
        self.user_frame_origin = None
        self.user_frame_yaw = None

    def exit(self):
        self.fcu.exit()

    def __str__(self):
        info = str(self.fcu)
        return info

    def __del__(self):
        self.exit()

    #########################################################################
    # middle layer functions
    #########################################################################
    def arm(self):
        """
        解锁无人机，在怠速下转动桨。
        """
        self.fcu.arm_disarm(True)

    def disarm(self):
        """
        无人机上锁，停止转动桨。
        """
        self.fcu.arm_disarm(False)

    def takeoff(self):
        """
        无人机起飞并悬停在指定高度，高度通过QGC设置。
        Takeoff
        """
        self.fcu.set_mode(PX4_FLIGHT_MODE.TAKEOFF)

    def land(self):
        """
        无人机原地降落。
        Land
        """
        self.fcu.set_mode(PX4_FLIGHT_MODE.LAND)

    def rtl(self):
        """
        无人机返回至起飞点。
        Return to launch
        """
        self.fcu.set_mode(PX4_FLIGHT_MODE.RETURN)

    def hold(self):
        """
        无人机在当前位置悬停。
        Keep still at current position
        """
        self.fcu.set_mode(PX4_FLIGHT_MODE.HOLD)

    def offboard_enable(self):
        """
        启动offboard模式，用于让无人机响应并执行接收到的控制指令。
        Enable offboard mode.
        """
        self.fcu.set_mode(PX4_FLIGHT_MODE.OFFBOARD)

    def set_mission(self, wps, takeoff_height=5):
        pass

    def start_mission(self):
        pass

    def clear_mission(self):
        pass

    #########################################################################
    # fly control in body frame
    #########################################################################
    def moveby_bodyNED_acc_rate(self, acc_x, acc_y, acc_z, yaw_rate):
        """
        机体坐标系下的飞行控制，设置机体坐标系下无人机飞行加速度和偏航转动角速度。\n
        Move with acceleration control, set accelerate and yaw rotate speed in body frame.\n

        Inputs:
            - acc_x: desired accelerate x in body frame(FRD)
            - acc_y: desired accelerate y in body frame(FRD)
            - acc_z: desired accelerate z in body frame(FRD)
            - yaw_rate: desired yaw rate in body frame(FRD)
        Outputs:
            None
        """
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_ACC_YAWRATE, acc_x, acc_y, acc_z, yaw_rate)

    def moveby_bodyNED_acc_yaw(self, acc_x, acc_y, acc_z, yaw):
        """
        机体坐标系下的飞行控制，设置机体坐标系下无人机飞行加速度和偏航转动角度。\n
        Move with acceleration control, set accelerate and yaw rotate angle in body frame.\n

        Inputs:
            - acc_x: desired accelerate x in body frame(FRD)
            - acc_y: desired accelerate y in body frame(FRD)
            - acc_z: desired accelerate z in body frame(FRD)
            - yaw:   desired yaw angle in body frame(FRD)
        Outputs:
            None
        """
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_ACC_YAW, acc_x, acc_y, acc_z, yaw)

    def moveby_bodyNED_vel_rate(self, vel_x, vel_y, vel_z, yaw_rate):
        """
        机体坐标系下的飞行控制，设置机体坐标系下无人机飞行速度和偏航转动角速度。\n
        Move with speed control, set velocity and yaw rotate speed in body frame.\n

        Inputs:
            - vel_x: desired velocity x in body frame(FRD)
            - vel_y: desired velocity y in body frame(FRD)
            - vel_z: desired velocity z in body frame(FRD)
            - yaw_rate: desired yaw rate in body frame(FRD)
        Outputs:
            None
        """
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, vel_x, vel_y, vel_z, yaw_rate)

    def moveby_bodyNED_vel_yaw(self, vel_x, vel_y, vel_z, yaw):
        """
        机体坐标系下的飞行控制，设置机体坐标系下无人机飞行速度和偏航角度。\n
        Move with speed control, set velocity and yaw rotate speed in body frame.\n

        Inputs:
            - vel_x: desired velocity x in body frame(FRD)
            - vel_y: desired velocity y in body frame(FRD)
            - vel_z: desired velocity z in body frame(FRD)
            - yaw: desired yaw in body frame(FRD)
        Outputs:
            None
        """
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAW, vel_x, vel_y, vel_z, yaw_rate)

    def fly_front(self, vel, vel_max=2, timeout=2):
        """
        设定飞行速度，沿机头方向朝前飞。该命令在**offboard**模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n

        Inputs:
            - vel: desired velocity
            - vel_max: max velocity
        Outputs:
            None 
        """
        if abs(vel) > vel_max:
            vel = vel_max * vel / abs(vel)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, vel, 0.0, 0.0, 0.0)

    def fly_back(self, vel, vel_max=2, timeout=2):
        """
        设定飞行速度，沿机尾方向朝后飞。该命令在**offboard**模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n

        Inputs:
            - vel: desired velocity
            - vel_max: max velocity
        Outputs:
            None 
        """
        if abs(vel) > vel_max:
            vel = vel_max * vel / abs(vel)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, -vel, 0.0, 0.0, 0.0)

    def fly_left(self, vel, vel_max=2, timeout=2):
        """
        设定飞行速度，沿水平方向朝左飞。该命令在**offboard**模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n

        Inputs:
            - vel: desired velocity
            - vel_max: max velocity
        Outputs:
            None 
        """
        if abs(vel) > vel_max:
            vel = vel_max * vel / abs(vel)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, 0.0, -vel, 0.0, 0.0)

    def fly_right(self, vel, vel_max=2, timeout=2):
        """
        设定飞行速度，沿水平方向朝右飞。该命令在**offboard**模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n

        Inputs:
            - vel: desired velocity
            - vel_max: max velocity
        Outputs:
            None 
        """
        if abs(vel) > vel_max:
            vel = vel_max * vel / abs(vel)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, 0.0, vel, 0.0, 0.0)

    def fly_higher(self, vel, vel_max=2, timeout=2):
        """
        设定飞行速度，沿垂直方向朝上飞。该命令在**offboard**模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n

        Inputs:
            - vel: desired velocity
            - vel_max: max velocity
        Outputs:
            None 
        """
        if abs(vel) > vel_max:
            vel = vel_max * vel / abs(vel)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, 0.0, 0.0, -vel, 0.0)

    def fly_lower(self, vel, vel_max=2, timeout=2):
        """
        设定飞行速度，沿垂直方向朝下飞。该命令在**offboard**模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n

        Inputs:
            - vel: desired velocity
            - vel_max: max velocity
        Outputs:
            None 
        """
        if abs(vel) > vel_max:
            vel = vel_max * vel / abs(vel)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, 0.0, 0.0, vel, 0.0)

    def turn_left(self, yaw_rate, rate_max=1.0):
        """
        设定转动角速度，机头左转向。该命令在**offboard**模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n

        Inputs:
            - yaw_rate: desired rate
            - rate_max: max turning rate
        Outputs:
            None 
        """
        if abs(yaw_rate) > rate_max:
            yaw_rate = rate_max * yaw_rate / abs(yaw_rate)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, 0.0, 0.0, 0.0, -yaw_rate)

    def turn_right(self, yaw_rate, rate_max=1.0):
        """
        设定转动角速度，机头右转向。该命令在**offboard**模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n

        Inputs:
            - yaw_rate: desired rate
            - rate_max: max turning rate
        Outputs:
            None 
        """
        if abs(yaw_rate) > rate_max:
            yaw_rate = rate_max * yaw_rate / abs(yaw_rate)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, 0.0, 0.0, 0.0, yaw_rate)

    def moveto_relAlt(self, height_des, dis_z_threshold=1, speed_z_max=2.0, P=1.0):
        """
        通过控制无人机飞行速度垂直移动至指定高度并反馈距离指定高度的差值，这个高度是相对高度，home点高度为0。\n
        该命令在offboard模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n
        Move to a specific height(relative altitude)with Onboard Computer in control, by setting vertical speed while keep yaw unchanged.\n

        Inputs:
            - height_des     : desired relative altitude fly to
            - dis_z_threshold: drone will stop fly while threshold reached
            - speed_z_max    : max speed in fly
            - P              : proportion ratio of calculate move speed with position delta
        Outputs:
            delta to height.
        """
        delta = height_des - self.fcu.alt_rel
        if delta < dis_z_threshold:
            return delta
        vz = P * delta
        if abs(vz) > speed_z_max:
            vz = speed_z_max * vz / abs(vz)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, 0, 0, -vz, 0)
        return delta

    #########################################################################
    # fly control in local frame
    #########################################################################
    def moveby_localNED_acc_rate(self, acc_x, acc_y, acc_z, yaw_rate):
        """
        机体坐标系下的飞行控制，设置机体坐标系下无人机飞行加速度和偏航转动角速度。\n
        Move with acceleration control, set accelerate and yaw rotate speed in local frame.\n

        Inputs:
            - acc_x: desired accelerate x in local frame(NED)
            - acc_y: desired accelerate y in local frame(NED)
            - acc_z: desired accelerate z in local frame(NED)
            - yaw_rate: desired yaw rate in local frame(NED), in [-pi, pi]
        Outputs:
            None
        """
        self.fcu.setpoint_local_ned(
            PX4_TYPE_MASK.MASK_NED_ACC_YAWRATE, acc_x, acc_y, acc_z, yaw_rate)

    def moveby_localNED_acc_yaw(self, acc_x, acc_y, acc_z, yaw):
        """
        机体坐标系下的飞行控制，设置机体坐标系下无人机飞行加速度和偏航转动角度。\n
        Move with acceleration control, set accelerate and yaw angle in local frame.\n

        Inputs:
            - acc_x: desired accelerate x in local frame(NED)
            - acc_y: desired accelerate y in local frame(NED)
            - acc_z: desired accelerate z in local frame(NED)
            - yaw:   desired yaw angle in local frame(NED)
        Outputs:
            None
        """
        self.fcu.setpoint_local_ned(
            PX4_TYPE_MASK.MASK_NED_ACC_YAW, acc_x, acc_y, acc_z, yaw)

    def moveby_localNED_vel_rate(self, vel_x, vel_y, vel_z, yaw_rate):
        """
        设置局部坐标系下无人机飞行速度和偏航转动角速度，局部坐标系原点为home点。由于只是速度控制，局部坐标系和大地坐标系的xyz轴方向一致，故也可以当做大地坐标系下的速度控制。\n
        该命令在offboard模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n
        Move with speed control, set velocity and yaw rotate speed in local frame.\n

        Inputs:
            - vel_x: velcity x in localNED frame
            - vel_y: velcity y in localNED frame
            - vel_z: velcity z in localNED frame
            - yaw_rate: yaw rate
        Outputs:
            None
        """
        self.fcu.setpoint_local_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, vel_x, vel_y, vel_z, yaw_rate)

    def goto_localPosition_pos_yaw(self, lp, heading_mode=1, heading=0, dis_xy_threshold=3, dis_z_threshold=1):
        """
        给定局部坐标系下无人机目标坐标和偏航角，由飞控控制无人机飞行至该目标点，局部坐标系原点为home点。\n
        Move to a specific local position with Onboard Computer in control, by setting local position.\n
        FCU handle speed control!\n

        Inputs:
            - lp: the desired global position going to fly
            - heading_mode: \n
                0: keep current heading still or not while flying\n
                1: turn heading face to gp\n
                2: set heading to a specify angle\n
            - heading:  in rad unit, in NED frame\n
                -1:   turn heading to gp\n
                >=0:  heading angle\n
        Outputs:
            None
        """
        x = lp[0]
        y = lp[1]
        z = lp[2]
        dis = calculate_distanceof_twolocalpoint_2d(
            self.fcu.local_position, lp)
        delta = lp[2] - self.fcu.local_pos_z
        if dis < dis_xy_threshold and abs(delta) < dis_z_threshold:
            return dis, delta
        if heading_mode == 0:
            yaw = self.fcu.yaw_NED
        elif heading_mode == 1:
            yaw = calculate_yawof_localpoints_ned(self.fcu.local_position, lp)
        elif heading_mode == 2:
            yaw = heading

        self.fcu.setpoint_local_ned(
            PX4_TYPE_MASK.MASK_NED_POS_YAW, x, y, z, yaw)
        return dis, delta

    def rotateto_yaw(self, yaw_des, rate_max=0.5, rate_min=0.02, delta_threshold=math.radians(0.2), P_r=1.0):
        """
        偏航角度控制，用户可以设置无人机偏航角速度以及转动快慢（P_r参数）。\n
        Turn drone to a yaw angle, by setting rotate rate while keep postition unchanged.\n

        Inputs:
            - yaw_des: desired yaw in NED
            - rate_max: maxmum rate while turning
            - rate_min: minimum rate while turning
            - P_r     : Proportion ratio
        Outputs:
            delta angle to yaw
        """
        yaw = self.fcu.attitude_NED[2]
        yaw_rel = calculate_delta_yaw(yaw, yaw_des)
        if abs(yaw_rel) < delta_threshold:
            return yaw_rel
        yaw_rate = P_r * yaw_rel
        if abs(yaw_rate) > rate_max:
            yaw_rate = rate_max * yaw_rate / abs(yaw_rate)
        if abs(yaw_rate) < rate_min:
            yaw_rate = rate_min * yaw_rate / abs(yaw_rate)
        # self.fcu.setpoint_body_ned(PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, 0.0, 0.0, 0.0, yaw_rate)
        self.fcu.setpoint_local_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAW, 0, 0, 0, yaw_des)
        return yaw_rel

    def turnto_yaw(self, yaw_des, delta_threshold=math.radians(0.2)):
        """
        偏航角度控制，偏航角速度用户无法控制，由飞控底层根据配置的参数决定。\n
        Turn drone to a yaw angle, by setting yaw angle while keep postition unchanged.\n

        Inputs:
            - yaw_des: desired yaw in NED frame
            - delta_threshold: the threshold to judge whethe reach or not
        Outpts:
            delta yaw
        """
        yaw = self.fcu.attitude_NED[2]
        yaw_rel = calculate_delta_yaw(yaw, yaw_des)
        if abs(yaw_rel) < delta_threshold:
            return yaw_rel
        # self.fcu.setpoint_body_ned(PX4_TYPE_MASK.MASK_NED_VEL_YAW, 0, 0, 0, yaw_des)
        self.fcu.setpoint_local_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAW, 0, 0, 0, yaw_des)
        return yaw_rel

    #########################################################################
    # fly control in global frame
    #########################################################################
    def moveto_globalPosition_relAlt_yaw(self, gp,
                                         heading_mode=1, heading=0,
                                         dis_xy_threshold=3, dis_z_threshold=1,
                                         speed_xy_max=3.0, speed_z_max=3.0, rate_max=0.5,
                                         P_v=2.0, P_r=1.0):
        """
        通过速度控制飞行至指定坐标点（其中高度代表相对高度），飞行过程中偏航角度有三种模式（保持不动、转到与航线方向一致、转到设定角度）。\n
        该命令在offboard模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n
        Move to a specific global position with Onboard Computer in control, by setting rel altitude yaw angle and moving speed.\n

        Inputs:
            - gp: (lat, lon, rel_alt), the desired global position and relative altitude going to fly
            - heading_mode: \n
                0: keep current heading still or not while flying\n
                1: turn heading face to gp\n
                2: set heading to a specify angle\n
            - heading:  in rad unit, in NED frame, yaw angle\n
                -1:   turn heading to gp\n
                >=0:  heading angle\n
            - dis_xy_threshold: threshold in xy plane
            - dis_z_threshold:  threshold in z plane
            - speed_xy_max: max speed in xy plane
            - speed_z_max: max speed in z plane
            - rate_max: max rotate rate of yaw
            - P_v: proportion ratio of calculate move speed with position delta
            - P_r: proportion ratio of calculate rotate speed with angle delta
        Outputs:
            - distance
            - delta angle
        """
        ###
        #                body-X  gp
        #                    ^   ^
        #                    |  /
        #                    |a/
        #                    |/______> body-Y

        # a  :  the angle between north and the vector from drone to position
        # dis:  distance of drone position to position
        # disx: distance's projection on x-axis in body frame
        # disy: distance's projection on y-axis in body frame
        # so (disx,disy) is vector in body frame, (dis, 0) is vector in gp frame
        # |disx|     | cos(a)   -sin(a) |       |dis|
        # |    |  =  |                  |   *   |   |
        # |dixy|     | sin(a)    cos(a) |       |0  |

        #
        dis = calculate_haversin_distance(self.fcu.global_position, gp)
        delta = gp[2] - self.fcu.alt_rel
        if dis < dis_xy_threshold and abs(delta) < dis_z_threshold:
            return dis, delta

        yaw = self.fcu.attitude_NED[2]
        yaw_des_v = calculate_yaw_accordingto_two_global_position(
            self.fcu.global_position, gp)
        # print('yaw:', yaw, 'yaw_des_v', yaw_des_v)
        # Keep heading unchanged
        if heading_mode == 0:
            yaw_des_r = yaw
        # Turing heading face to gp
        elif heading_mode == 1:
            yaw_des_r = yaw_des_v
        # Set heading to specify angle
        elif heading_mode == 2:
            yaw_des_r = heading

        yaw_delta_v = calculate_delta_yaw(yaw, yaw_des_v)
        yaw_delta_r = calculate_delta_yaw(yaw, yaw_des_r)

        dis_x = math.cos(yaw_delta_v) * dis
        dis_y = math.sin(yaw_delta_v) * dis
        vel_x = P_v * dis_x
        vel_y = P_v * dis_y

        # adjust velocity
        if abs(vel_x) > abs(vel_y):
            ratio = abs(vel_x) / abs(vel_y)
            if abs(vel_x) > speed_xy_max:
                vel_x = speed_xy_max * vel_x / abs(vel_x)
                vel_y = abs(vel_x) / ratio * vel_y / abs(vel_y)
        elif abs(vel_x) <= abs(vel_y):
            ratio = abs(vel_y) / abs(vel_x)
            if abs(vel_y) > speed_xy_max:
                vel_y = speed_xy_max * vel_y / abs(vel_y)
                vel_x = abs(vel_y) / ratio * vel_x / abs(vel_x)
        ###
        rate_yaw = P_r * yaw_delta_r
        if abs(rate_yaw) > rate_max:
            rate_yaw = rate_max * rate_yaw / abs(rate_yaw)

        vel_z = P_v * delta
        if abs(vel_z) > speed_z_max:
            vel_z = speed_z_max * vel_z / abs(vel_z)

        if dis < dis_xy_threshold:
            vel_x = 0
            vel_y = 0
            rate_yaw = 0
        if abs(delta) < dis_z_threshold:
            vel_z = 0
        if yaw_delta_r > 3.0*math.pi/180:
            vel_x = 0
            vel_y = 0
            vel_z = 0
        # print(vel_x, vel_y, yaw_delta_r, dis)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, vel_x, vel_y, -vel_z, rate_yaw)
        return dis, delta

    def moveto_globalPosition_alt_yaw(self, gp,
                                      heading_mode=1, heading=0,
                                      dis_xy_threshold=3, dis_z_threshold=1,
                                      speed_xy_max=3.0, speed_z_max=3.0, rate_max=0.5,
                                      P_v=2.0, P_r=1.0):
        """
        通过速度控制飞行至指定坐标点（其中高度代表绝对高度），飞行过程中偏航角度有三种模式（保持不动、转到与航线方向一致、转到设定角度）。\n
        该命令在offboard模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n
        Move to a specific global position with Onboard Computer in control, by setting altitude yaw angle and moving speed.\n

        Inputs:
            - gp: (lat, lon, alt), the desired global position going to fly
            - heading_mode: \n
                0: keep current heading still or not while flying\n
                1: turn heading face to gp\n
                2: set heading to a specify angle\n
            - heading:  in rad unit, in NED frame, yaw angle\n
                -1:   turn heading to gp\n
                >=0:  heading angle\n
            - dis_xy_threshold: threshold in xy plane
            - dis_z_threshold:  threshold in z plane
            - speed_xy_max: max speed in xy plane
            - speed_z_max: max speed in z plane
            - rate_max: max rotate rate of yaw
            - P_v: proportion ratio of calculate move speed with position delta
            - P_r: proportion ratio of calculate rotate speed with angle delta
        Outputs:
            None
        """
        ###
        #                body-X  gp
        #                    ^   ^
        #                    |  /
        #                    |a/
        #                    |/______> body-Y

        # a  :  the angle between north and the vector from drone to position
        # dis:  distance of drone position to position
        # disx: distance's projection on x-axis in body frame
        # disy: distance's projection on y-axis in body frame
        # so (disx,disy) is vector in body frame, (dis, 0) is vector in gp frame
        # |disx|     | cos(a)   -sin(a) |       |dis|
        # |    |  =  |                  |   *   |   |
        # |dixy|     | sin(a)    cos(a) |       |0  |
        #
        dis = calculate_haversin_distance(self.fcu.global_position, gp)
        delta = gp[2] - self.fcu.altitude
        if dis < dis_xy_threshold and abs(delta) < dis_z_threshold:
            return dis, delta

        yaw = self.fcu.attitude_NED[2]
        yaw_des_v = calculate_yaw_accordingto_two_global_position(
            self.fcu.global_position, gp)
        # print('yaw:', yaw, 'yaw_des_v', yaw_des_v)
        # Keep heading unchanged
        if heading_mode == 0:
            yaw_des_r = yaw
        # Turing heading face to gp
        elif heading_mode == 1:
            yaw_des_r = yaw_des_v
        # Set heading to specify angle
        elif heading_mode == 2:
            yaw_des_r = heading

        yaw_delta_v = calculate_delta_yaw(yaw, yaw_des_v)
        yaw_delta_r = calculate_delta_yaw(yaw, yaw_des_r)

        dis_x = math.cos(yaw_delta_v) * dis
        dis_y = math.sin(yaw_delta_v) * dis
        vel_x = P_v * dis_x
        vel_y = P_v * dis_y

        # adjust velocity
        if abs(vel_x) > abs(vel_y):
            ratio = abs(vel_x) / abs(vel_y)
            if abs(vel_x) > speed_xy_max:
                vel_x = speed_xy_max * vel_x / abs(vel_x)
                vel_y = abs(vel_x) / ratio * vel_y / abs(vel_y)
        elif abs(vel_x) <= abs(vel_y):
            ratio = abs(vel_y) / abs(vel_x)
            if abs(vel_y) > speed_xy_max:
                vel_y = speed_xy_max * vel_y / abs(vel_y)
                vel_x = abs(vel_y) / ratio * vel_x / abs(vel_x)
        ###
        rate_yaw = P_r * yaw_delta_r
        if abs(rate_yaw) > rate_max:
            rate_yaw = rate_max * rate_yaw / abs(rate_yaw)

        vel_z = P_v * delta
        if abs(vel_z) > speed_z_max:
            vel_z = speed_z_max * vel_z / abs(vel_z)

        if dis < dis_xy_threshold:
            vel_x = 0
            vel_y = 0
            rate_yaw = 0
        if abs(delta) < dis_z_threshold:
            vel_z = 0
        if yaw_delta_r > 3.0*math.pi/180:
            vel_x = 0
            vel_y = 0
            vel_z = 0
        # print(vel_x, vel_y, yaw_delta_r, dis)
        self.fcu.setpoint_body_ned(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, vel_x, vel_y, -vel_z, rate_yaw)
        return dis, delta

    def moveby_globalNED_vel_rate(self, vel_lat, vel_lon, vel_alt, yaw_rate):
        """
        Not success yet
        """
        self.fcu.setpoint_global_ned_alt(
            PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE, vel_lat, vel_lon, vel_alt, yaw_rate)

    def goto_globalPosition_relAlt(self, gp, heading_mode=1, heading=0):
        """
        Not success yet
        """
        lat = gp[0]
        lon = gp[1]
        rel_alt = gp[2]
        dis = calculate_haversin_distance(self.fcu.global_position, gp)
        self.fcu.setpoint_global_ned_alt(
            PX4_TYPE_MASK.MASK_NED_POS_YAW, lat, lon, rel_alt, heading)
        return dis

    def goto_relAlt_async(self, height_des, speed=2):
        """
        Goto a relative height with a specified speed async.\n   
        Inputs:
            - height_des: relative height
            - speed: flying speed
        Outputs:
            None
        """
        gp = (self.fcu.latitude, self.fcu.longitude, height_des)
        self.goto_globalPosition_relAlt_yaw_async(
            gp, heading_mode=0, heading=0, speed=speed)

    def goto_globalPosition_relAlt_yaw_async(self, gp, heading_mode=1, heading=0, speed=2):
        """
        设定一个global坐标系下位置坐标（其中高度代表相对高度）和朝向角度，发送给无人机，由无人机自主飞行至该位置，不需要offboard模式。\n
        Goto a global position and relative altitude async, by setting heading angle(yaw) and speed.\n
        Inputs:
            - gp: the desired global position going fly to
            - heading_mode: \n
                0: keep current heading still or not while flying\n
                1: turn heading face to gp\n
                2: set heading to a specify angle\n
            - heading:  in rad unit, in NED frame\n
                -1:   turn heading to gp\n
                >=0:  heading angle\n
            - speed: flying speed
        Outputs:
            None
        """
        lat = gp[0]
        lon = gp[1]
        alt = gp[2] + self.fcu.home_altitude

        if heading_mode == 0:
            yaw = self.fcu.yaw_NED * 180.0 / math.pi
        elif heading_mode == 1:
            yaw = calculate_yaw_accordingto_two_global_position(
                self.fcu.global_position, gp)
        elif heading_mode == 2:
            yaw = heading * 180.0 / math.pi

        self.fcu.cmd_goto_global_position(lat, lon, alt, yaw, speed)

    def goto_globalPosition_alt_yaw_async(self, gp, heading_mode=1, heading=0, speed=2):
        """
        设定一个global坐标系下位置坐标（其中高度代表绝对高度）和朝向角度，发送给无人机，由无人机自主飞行至该位置，不需要offboard模式。\n
        Goto a global position and absolute altitude async, by setting heading angle(yaw) and speed.\n

        Inputs:
            - gp: the desired global position going fly to
            - heading_mode: \n
                0: keep current heading still or not while flying\n
                1: turn heading face to gp\n
                2: set heading to a specify angle\n
            - heading:  in rad unit, in NED frame\n
                -1:   turn heading to gp\n
                >=0:  heading angle\n
            - speed: flying speed
        Outputs:
            None
        """
        lat = gp[0]
        lon = gp[1]
        alt = gp[2]
        if heading_mode == 0:
            yaw = self.fcu.yaw_NED * 180.0 / math.pi
        elif heading_mode == 1:
            yaw = calculate_yaw_accordingto_two_global_position(
                self.fcu.global_position, gp)
        elif heading_mode == 2:
            yaw = heading * 180.0 / math.pi

        self.fcu.cmd_goto_global_position(lat, lon, alt, yaw, speed)

    #########################################################################
    # fly control in user frame
    #########################################################################
    def set_user_frame(self, origin_user, yaw):
        """
        设置用户坐标系，只需要给定用户坐标系原点对应的经纬度以及用户坐标系与大地坐标系NED的偏转角度即可。\n
        set user frame before you can fly control in it!\n

        Inputs:
            - origin_user: (lat, lon, rel_alt), user frame origin in GPS position
            - yaw        : rotate angle between global frame and user frame, in [-pi, pi]
        Outputs:
            None
        """
        self.user_frame_origin = origin_user
        self.user_frame_yaw = yaw

    def get_user_position(self):
        """
        获取无人机在用户坐标系下的坐标（必须先设置用户坐标系才能调用）。\n
        Inputs:
            None
        Outputs:
            (x, y, z), Position in user frame
        """
        up = (0.0, 0.0, 0.0)
        try:
            up = convert_position_global2user(self.user_frame_origin, self.user_frame_yaw, (
                self.fcu.latitude, self.fcu.longitude, self.fcu.altitude))
        except Exception as e:
            print(f"ERROR: drone_api.py, get_user_position.{e}")
        return up

    def get_user_velocity(self):
        """
        获取无人机在用户坐标系下的速度（必须先设置用户坐标系才能调用）。\n
        Inputs:
            None
        Outputs:
            (vx, vy, vz), Velocity in user frame
        """
        # 将无人机在global坐标系下的速度转成用户坐标系下的速度。
        # |u_x|     | cos(a)    sin(a) |       |g_x|
        # |   |  =  |                  |   *   |   |
        # |u_y|     |-sin(a)    cos(a) |       |g_y|
        #  u_z   =                             -g_z
        # vel_x =  math.cos(self.user_frame_yaw) * self.fcu.global_vel_x + math.sin(self.user_frame_yaw) * self.fcu.global_vel_y
        # vel_y = -math.sin(self.user_frame_yaw) * self.fcu.global_vel_x + math.cos(self.user_frame_yaw) * self.fcu.global_vel_y
        # vel_z = -self.fcu.global_vel_z
        vel_u = convert_vector_global2user(
            (self.fcu.global_vel_x, self.fcu.global_vel_y, self.fcu.global_vel_z), self.user_frame_yaw)

        return vel_u

    def moveby_user_acc_rate(self, acc_x, acc_y, acc_z, yaw_rate):
        """
        设置用户坐标系下无人机飞行速度和偏航转动角速度。\n
        该命令在offboard模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n
        Move with acceleration control, set accelerate and yaw rotate speed in user frame.\n
        Inputs:
            acc_x: accelerate x in user frame
            acc_y: accelerate y in user frame
            acc_z: accelerate z in user frame
            yaw_rate: yaw rate
        Outputs:
            None
        """

        # 将无人机在用户坐标系下的速度转成在局部NED坐标系下的加速度。
        # |l_x|     | cos(a)   -sin(a) |       |u_x|
        # |   |  =  |                  |   *   |   |
        # |l_y|     | sin(a)    cos(a) |       |u_y|
        acc_x_local = math.cos(self.user_frame_yaw) * \
            acc_x - math.sin(self.user_frame_yaw) * acc_y
        acc_y_local = math.sin(self.user_frame_yaw) * \
            acc_x + math.cos(self.user_frame_yaw) * acc_y
        self.moveby_localNED_acc_rate(
            acc_x_local, acc_y_local, acc_z, yaw_rate)

    def moveby_user_acc_yaw(self, acc_x, acc_y, acc_z, yaw):
        """
        设置用户坐标系下无人机飞行速度和偏航转动角速度。\n
        该命令在offboard模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n
        Move with acceleration control, set accelerate and yaw rotate angle in user frame.\n
        Inputs:
            acc_x: accelerate x in user frame
            acc_y: accelerate y in user frame
            acc_z: accelerate z in user frame
            yaw:   yaw angle, in [-pi, pi]
        Outputs:
            None
        """

        # 将无人机在用户坐标系下的速度转成在局部NED坐标系下的加速度。
        # |l_x|     | cos(a)   -sin(a) |       |u_x|
        # |   |  =  |                  |   *   |   |
        # |l_y|     | sin(a)    cos(a) |       |u_y|
        acc_x_local = math.cos(self.user_frame_yaw) * \
            acc_x - math.sin(self.user_frame_yaw) * acc_y
        acc_y_local = math.sin(self.user_frame_yaw) * \
            acc_x + math.cos(self.user_frame_yaw) * acc_y

        yaw_local = self.user_frame_yaw + yaw
        if yaw_local > math.pi:
            yaw_local -= 2 * math.pi
        if yaw_local < -math.pi:
            yaw_local += 2*math.pi
        self.moveby_localNED_acc_yaw(
            acc_x_local, acc_y_local, acc_z, yaw_local)

    def moveby_user_vel_rate(self, vel_x, vel_y, vel_z, yaw_rate):
        """
        设置用户坐标系下无人机飞行速度和偏航转动角速度。\n
        该命令在offboard模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n
        Move with speed control, set velocity and yaw rotate speed in user frame.\n
        Inputs:
            vel_x: velcity x in user frame
            vel_y: velcity y in user frame
            vel_z: velcity z in user frame
            yaw_rate: yaw rate
        Outputs:
            None
        """

        # 将无人机在用户坐标系下的速度转成在局部NED坐标系下的速度。
        # |l_x|     | cos(a)   -sin(a) |       |u_x|
        # |   |  =  |                  |   *   |   |
        # |l_y|     | sin(a)    cos(a) |       |u_y|
        vel_x = math.cos(self.user_frame_yaw) * vel_x - \
            math.sin(self.user_frame_yaw) * vel_y
        vel_y = math.sin(self.user_frame_yaw) * vel_x + \
            math.cos(self.user_frame_yaw) * vel_y
        self.moveby_localNED_vel_rate(vel_x, vel_y, vel_z, yaw_rate)

    def moveby_user_vel_yaw(self, vel_x, vel_y, vel_z, yaw):
        """
        设置用户坐标系下无人机飞行速度和偏航角度。\n
        该命令在offboard模式下有效，需要至少10Hz频率发送，命令停止发送无人机飞行停止。\n
        Move with speed control, set velocity and yaw angle in user frame.\n

        Inputs:
            vel_x: velcity x in user frame
            vel_y: velcity y in user frame
            vel_z: velcity z in user frame
            yaw: yaw
        Outputs:
            None
        """

        # 将无人机在用户坐标系下的速度转成在局部NED坐标系下的速度。
        # |l_x|     | cos(a)   -sin(a) |       |u_x|
        # |   |  =  |                  |   *   |   |
        # |l_y|     | sin(a)    cos(a) |       |u_y|
        vel_x = math.cos(self.user_frame_yaw) * vel_x - \
            math.sin(self.user_frame_yaw) * vel_y
        vel_y = math.sin(self.user_frame_yaw) * vel_x + \
            math.cos(self.user_frame_yaw) * vel_y
        self.moveby_localNED_vel_rate(vel_x, vel_y, vel_z, yaw)

    def goto_userPosition_relAlt_yaw_async(self, up, heading_mode=0, heading=0, speed=2):
        """
        设定一个user坐标系下位置坐标（其中高度代表绝对高度）和朝向角度，发送给无人机，由无人机自主飞行至该位置，不需要offboard模式。\n
        Goto a position and absolute altitude async in user frame, by setting heading angle(yaw) and speed.\n
        Inputs:
            - up: the desired position in user frame going fly to
            - heading_mode: \n
                0: keep current heading still or not while flying\n
                1: turn heading face to gp\n
                2: set heading to a specify angle\n
            - heading:  in rad unit, in NED frame\n
                -1:   turn heading to gp\n
                >=0:  heading angle\n
            - speed: flying speed
        Outputs:
            None
        """
        gp = convert_position_user2global(
            self.user_frame_origin, self.user_frame_yaw, pos)
        self.goto_globalPosition_relAlt_yaw_async(
            gp, heading_mode=heading_mode, heading=heading, speed=speed)
